

import java.awt.Graphics;
import java.awt.Paint;
import java.util.ArrayList;

public class RigidBody {

	Boolean isFixed;

	ydPoint position;
	ydPoint p2;
	ydPoint p3;
	ydPoint p4;
	
	ydPoint pivot;

	ydPoint center_mass;

	ydPoint velocity;
	ydPoint acceleration;

	ydForce total_uniform_force;

	ydPoint endydPoints;

	float angular_velocity;
	float MASS;

	ydPoint size;

	float theta;
	
	Boolean shouldMove;

	public RigidBody() {
		isFixed = false;

		position = new ydPoint();
		p2 = new ydPoint();
		p3 = new ydPoint();
		p4 = new ydPoint();

		center_mass = new ydPoint();

		pivot=center_mass;
		
		velocity = new ydPoint();
		acceleration = new ydPoint();

		total_uniform_force = new ydForce(new ydPoint(), new ydPoint(), true);

		endydPoints = new ydPoint();

		size = new ydPoint();

		angular_velocity = 0;

		MASS = 0;

		theta = 0;
		
		shouldMove=true;

	}

	public void setIsFixed(Boolean isFixed) {
		this.isFixed = isFixed;

		velocity = new ydPoint();
		acceleration = new ydPoint();

	}

	public RigidBody(RigidBody r) {
		isFixed = r.isFixed;

		position = new ydPoint(r.position);
		p2 = new ydPoint(r.p2);
		p3 = new ydPoint(r.p3);
		p4 = new ydPoint(r.p4);

		center_mass = new ydPoint(r.center_mass);

		pivot=center_mass;
		
		velocity = new ydPoint(r.velocity);
		acceleration = new ydPoint(r.acceleration);

		total_uniform_force = new ydForce(new ydPoint(
				r.total_uniform_force.position), new ydPoint(
				r.total_uniform_force.magnitude), true);

		endydPoints = new ydPoint(r.endydPoints);

		size = new ydPoint(r.size);

		angular_velocity = r.angular_velocity;

		MASS = r.MASS;
		
		shouldMove=true;
	}

	public void calculateNextPosition(ydPoint a) {
		if (isFixed)
			return;
		
		if(!shouldMove)
			return;

		theta += angular_velocity;

		calculateAcceleration();
		calculateVelocity();
		calculatePosition();
		
		//calculateAngularVelocityTotal();

	}
	
	public void shouldMove(Boolean s){
		shouldMove=s;
	}

	public void calculateAcceleration() {
		acceleration.x = total_uniform_force.magnitude.x / MASS;
		acceleration.y = total_uniform_force.magnitude.y / MASS;

	}

	public void calculateVelocity() {
		velocity.x += acceleration.x * 0.5;
		velocity.y += acceleration.y * 0.5;
	}

	public void calculatePosition() {
		center_mass.x += velocity.x;
		center_mass.y += velocity.y;

		position.x += velocity.x;
		position.y += velocity.y;

		p2.x += velocity.x;
		p2.y += velocity.y;

		p3.x += velocity.x;
		p3.y += velocity.y;

		p4.x += velocity.x;
		p4.y += velocity.y;

	}

	public void calculateAngularVelocityTotal() {
		double gravity = 9.81;

		double delta = 0;

		angular_velocity = 0;
		
		// calculateCenterMass();

		delta = pivot.x - position.x;

		angular_velocity += (gravity * MASS / 4) * delta;

		delta = pivot.x - p2.x;

		angular_velocity += (gravity * MASS / 4) * delta;

		delta = pivot.x - p3.x;

		angular_velocity += (gravity * MASS / 4) * delta;

		delta = pivot.x - p4.x;

		angular_velocity += (gravity * MASS / 4) * delta;

		angular_velocity *= -0.000001;

		rotateAround(new ydPoint(pivot));

	}

	public void calcCornerPoints() {

		double angle2 = Globals.degreesToRads(90) - theta;

		p2.x = (int) (position.x + size.y * Math.cos(angle2));
		p2.y = (int) (position.y + size.y * Math.sin(angle2));

		p4.x = (int) (p2.x + size.x * Math.cos(theta));
		p4.y = (int) (p2.y - size.x * Math.sin(theta));

		p3.x = (int) (position.x + size.x * Math.cos(theta));
		p3.y = (int) (position.y - size.x * Math.sin(theta));

		calculateCenterMass();
	}

	public void rotateydPointAround(ydPoint ydPoint, ydPoint Pivot, double rads) {
		ydPoint displacement = new ydPoint(ydPoint.x - Pivot.x, ydPoint.y
				- Pivot.y);

		ydPoint.x = (int) (Pivot.x + displacement.x * Math.cos(rads) + displacement.y
				* Math.sin(rads));
		ydPoint.y = (int) (Pivot.y + displacement.y * Math.cos(rads) - displacement.x
				* Math.sin(rads));
		return;

	}

	public void rotateAround(ydPoint Pivot) {

		position.rotatePointAround(Pivot, angular_velocity);
		p2.rotatePointAround(Pivot, angular_velocity);
		p3.rotatePointAround(Pivot, angular_velocity);
		p4.rotatePointAround(Pivot, angular_velocity);

		/*
		 * rotate_ydPoint(Pivot.x, Pivot.y,(float) theta, position);
		 * rotate_ydPoint(Pivot.x, Pivot.y,(float) theta, p2);
		 * rotate_ydPoint(Pivot.x, Pivot.y,(float) theta, p3);
		 * rotate_ydPoint(Pivot.x, Pivot.y,(float) theta, p4);
		 */

		/*
		 * rotateydPointAround(position, Pivot, theta); rotateydPointAround(p2,
		 * Pivot, theta); rotateydPointAround(p3, Pivot, theta);
		 * rotateydPointAround(p4, Pivot, theta);
		 */

		/*
		 * double angle2 = Globals.degreesToRads(90) - theta;
		 * 
		 * ydPoint tmp = new ydPoint(position);
		 * 
		 * translateBody(new ydPoint(Pivot.x-position.x, Pivot.y-position.y));
		 * 
		 * p2.x = (int) (position.x + size.y * Math.cos(angle2)); p2.y = (int)
		 * (position.y + size.y * Math.sin(angle2));
		 * 
		 * p4.x = (int) (p2.x + size.x * Math.cos(theta)); p4.y = (int) (p2.y -
		 * size.x * Math.sin(theta));
		 * 
		 * p3.x = (int) (position.x + size.x * Math.cos(theta)); p3.y = (int)
		 * (position.y - size.x * Math.sin(theta));
		 * 
		 * translateBody(new ydPoint(tmp.x-position.x, tmp.y-position.y));
		 * 
		 * ydPoint CM1=new ydPoint(center_mass);
		 * 
		 * calculateCenterMass();
		 * 
		 * translateBody(new ydPoint(CM1.x-center_mass.x, CM1.y-center_mass.y));
		 */

	}

	public void translateBody(ydPoint shift) {

		center_mass.x += shift.x;
		center_mass.y += shift.y;

		position.x += shift.x;
		position.y += shift.y;

		p2.x += shift.x;
		p2.y += shift.y;

		p3.x += shift.x;
		p3.y += shift.y;

		p4.x += shift.x;
		p4.y += shift.y;

	}

	public void translateBodyTo(ydPoint shift) {

		position.x = shift.x;
		position.y = shift.y;

		calcCornerPoints();

	}

	public void calculateCenterMass() {
		center_mass.x = (p4.x - position.x) / 2 + position.x;
		center_mass.y = (p4.y - position.y) / 2 + position.y;
	}

	public void drawBody(Graphics g) {

		/*
		 * canvas.drawText("P1", position.x, position.y, paint);
		 * canvas.drawText("P2", p2.x, p2.y, paint); canvas.drawText("P3", p3.x,
		 * p3.y, paint); canvas.drawText("P4", p4.x, p4.y, paint);
		 */
		
		g.drawLine((int)position.x, (int)position.y, (int)p3.x, (int)p3.y);
		g.drawLine((int)p3.x, (int)p3.y, (int)p4.x, (int)p4.y);
		g.drawLine((int)p4.x, (int)p4.y, (int)p2.x, (int)p2.y);
		g.drawLine((int)p2.x, (int)p2.y, (int)position.x, (int)position.y);

		// canvas.drawPoint(center_mass.x, center_mass.y, paint);

		/*
		 * ydPoint p=new ydPoint(200,425);
		 * 
		 * canvas.drawPoint(p.x, p.y, paint);
		 * 
		 * canvas.drawText("Theta = " + theta + "", 20, 40, paint);
		 * canvas.drawText("Angular Velocity = " + angular_velocity + "", 20,
		 * 60, paint);
		 * 
		 * canvas.drawText("P1: " +position.toString()+ "", 20, 80, paint);
		 * canvas.drawText("P2: " +p2.toString()+ "", 20, 90, paint);
		 * canvas.drawText("P3: " +p3.toString()+ "", 20, 100, paint);
		 * canvas.drawText("P4: " +p4.toString()+ "", 20, 110, paint);
		 */

		// canvas.drawText("CM: " +center_mass.toString()+ "", 20, 120, paint);
	}

	public void addForce(ydForce force) {
		/*
		 * total_uniform_force.magnitude.x=0; total_uniform_force.magnitude.y=0;
		 */
		if (force.isUniform) {
			addUniformForce(force);
		}

	}

	public void addUniformForce(ydForce force) {
		total_uniform_force.magnitude.x += force.magnitude.x;
		total_uniform_force.magnitude.y += force.magnitude.y;
	}

	public Boolean doesIntersect(ydPoint p) {

		return false;
	}

	ydLine l1 = new ydLine(new ydPoint(0.1f, -1), new ydPoint(100, 45));

/*	public static void collisionOccured2(RigidBody A, RigidBody B, Canvas c,
			Paint p) {

		ydPoint slopeVector = slopeOf(B.center_mass, A.center_mass);
		float slopeDegrees = vectorToDegrees(slopeVector);
		slopeVector.degreesToVector(slopeDegrees);

		
		 * c.drawText(slopeDegrees + "", 20, 75, p);
		 * c.drawText(slopeVector.toString(), 20, 85, p);
		 

		ydLine aL1 = new ydLine(slopeVector, A.center_mass);
		ydLine aL2 = new ydLine(slopeOf(A.p2, A.p4), A.p2);

		ydLine bL1 = new ydLine(new ydPoint(-slopeVector.x, -slopeVector.y),
				B.center_mass);
		ydLine bL2 = new ydLine(slopeOf(B.position, B.p2), B.position);

		
		 * aL1.drawLine(c, p); // aL2.drawLine(c, p);
		 * 
		 * bL1.drawLine(c, p); // bL2.drawLine(c, p);
		 
		ydPoint intesects = linesIntectAt(aL1, aL2);
		ydPoint intesects2 = linesIntectAt(bL1, bL2);

		
		 * c.drawLine(A.center_mass.x, A.center_mass.y, B.center_mass.x,
		 * B.center_mass.y, p); c.drawCircle(intesects.x, intesects.y, 5, p);
		 * c.drawCircle(intesects2.x, intesects2.y, 5, p);
		 

		
		 * ydLine l1=new ydLine(new ydPoint(0.1f,-1), new ydPoint(100, 45));
		 * ydLine l2=new ydLine(new ydPoint(-3.5f,1), new ydPoint(466, 350));
		 * 
		 * ydPoint i=linesIntectAt(l1, l2);
		 * 
		 * l1.drawLine(c, p); l2.drawLine(c, p);
		 * 
		 * i.drawPoint(c, p);
		 

	}*/

	private static float vectorToDegrees(ydPoint slopeVector) {

		if (slopeVector.y == 0)
			if (slopeVector.x > 0)
				return 0;
			else
				return 180;

		if (slopeVector.x == 0)
			if (slopeVector.y < 0)
				return 90;
			else
				return 270;

		float extra = 0;

		if (slopeVector.x >= 0 && slopeVector.y < 0) {
			extra = 0;
			slopeVector.y *= -1;
		} else if (slopeVector.x < 0 && slopeVector.y <= 0) {
			float tmp = slopeVector.y;
			slopeVector.x *= -1;
			slopeVector.y = slopeVector.x;
			slopeVector.x = -tmp;
			extra = 90;
		} else if (slopeVector.x <= 0 && slopeVector.y > 0) {
			float tmp = slopeVector.y;
			slopeVector.x *= -1;
			// slopeVector.y=slopeVector.x;
			// slopeVector.x=tmp;
			extra = 180;
		} else if (slopeVector.x > 0 && slopeVector.y > 0) {
			float tmp = slopeVector.y;
			slopeVector.x *= -1;
			slopeVector.y = slopeVector.x;
			slopeVector.x = -tmp;
			extra = 270;
		}

		return Globals.radsToDegrees((float) Math.atan(slopeVector.y
				/ slopeVector.x))
				+ extra;
	}

/*	public static Boolean collisionOccured(RigidBody A, RigidBody B, Canvas c,
			Paint p) {

		// x holds overlap, y holds angle in degress
		ydPoint minOverLapSlope = new ydPoint(10000, 10000);

		if (SAT(A, B, c, p, B.position, B.p2, minOverLapSlope))
			return false;
		if (SAT(A, B, c, p, B.position, B.p3, minOverLapSlope))
			return false;
		if (SAT(A, B, c, p, B.p3, B.p4, minOverLapSlope))
			return false;
		if (SAT(A, B, c, p, B.p2, B.p4, minOverLapSlope))
			return false;

		if (SAT(A, B, c, p, A.position, A.p2, minOverLapSlope))
			return false;
		if (SAT(A, B, c, p, A.position, A.p3, minOverLapSlope))
			return false;
		if (SAT(A, B, c, p, A.p3, A.p4, minOverLapSlope))
			return false;
		if (SAT(A, B, c, p, A.p2, A.p4, minOverLapSlope))
			return false;

		
		 * c.drawText(minOverLapSlope.x + "          " + minOverLapSlope.y, 50,
		 * 100, p);
		 

		ydPoint s = (new ydPoint());
		s.degreesToVector(minOverLapSlope.y);
		ydLine l = new ydLine(s, new ydPoint(400, 400));
		l.drawLine(c, p);
		ydPoint v = slopeOf(A.center_mass, B.center_mass);

		
		 * s.x*=5; s.y*=5;
		 
		if (s.x < 0) {
			s.x *= -1;
			s.y *= -1;

		}

		ydForce totalForce = new ydForce(new ydPoint(), new ydPoint(
				A.total_uniform_force.magnitude), true);
		totalForce.magnitude.x += B.total_uniform_force.magnitude.x
				+ A.acceleration.x * A.MASS + B.acceleration.x * B.MASS;
		totalForce.magnitude.y += B.total_uniform_force.magnitude.y
				+ A.acceleration.x * A.MASS + B.acceleration.x * B.MASS;

		s.normailize();

		s.x *= totalForce.magnitude.x;

		s.y *= totalForce.magnitude.y;

		if (A.center_mass.x < B.center_mass.x) {
			A.velocity.x = -s.x;
			A.velocity.y = -s.x;

			B.velocity.x = s.x;
			B.velocity.y = s.x;
		} else {
			A.velocity.x = s.x;
			A.velocity.y = s.x;

			B.velocity.x = -s.x;
			B.velocity.y = -s.x;
		}

		return true;

	}*/

	public static Boolean collisionOccured(RigidBody A, RigidBody B) {

		// x holds overlap, y holds angle in degress
		ydPoint minOverLapSlope = new ydPoint(10000, 10000);

		if (SAT(A, B, B.position, B.p2, minOverLapSlope))
			return false;
		if (SAT(A, B, B.position, B.p3, minOverLapSlope))
			return false;
		if (SAT(A, B, B.p3, B.p4, minOverLapSlope))
			return false;
		if (SAT(A, B, B.p2, B.p4, minOverLapSlope))
			return false;

		if (SAT(A, B, A.position, A.p2, minOverLapSlope))
			return false;
		if (SAT(A, B, A.position, A.p3, minOverLapSlope))
			return false;
		if (SAT(A, B, A.p3, A.p4, minOverLapSlope))
			return false;
		if (SAT(A, B, A.p2, A.p4, minOverLapSlope))
			return false;

		/*
		 * c.drawText(minOverLapSlope.x + "          " + minOverLapSlope.y, 50,
		 * 100, p);
		 */

		ydPoint s = (new ydPoint());
		s.degreesToVector(minOverLapSlope.y);
		ydLine l = new ydLine(s, new ydPoint(400, 400));
		ydPoint v = slopeOf(A.center_mass, B.center_mass);

		/*
		 * s.x*=5; s.y*=5;
		 */
		
	/*	if(true){
			A.shouldMove(false);
			B.shouldMove(false);
		}*/
		
		if (s.x < 0) {
			s.x *= -1;
			s.y *= -1;

		}

		ydForce totalForce = new ydForce(new ydPoint(), new ydPoint(
				A.total_uniform_force.magnitude), true);
		totalForce.magnitude.x += B.total_uniform_force.magnitude.x
				+ A.acceleration.x * A.MASS + B.acceleration.x * B.MASS;
		totalForce.magnitude.y += B.total_uniform_force.magnitude.y
				+ A.acceleration.x * A.MASS + B.acceleration.x * B.MASS;

		s.normailize();

		s.x *= totalForce.magnitude.x;

		s.y *= totalForce.magnitude.y;

		if (A.center_mass.x < B.center_mass.x) {
			A.velocity.x = -s.x;
			A.velocity.y = -s.x;

			B.velocity.x = s.x;
			B.velocity.y = s.x;
		} else {
			A.velocity.x = s.x;
			A.velocity.y = s.x;

			B.velocity.x = -s.x;
			B.velocity.y = -s.x;
		}
		
		A.velocity.x = 0;
		A.velocity.y = 0.1f;

		B.velocity.x = 0;
		B.velocity.y = 0.1f;

		return true;

	}

/*	private static Boolean SAT(RigidBody A, RigidBody B, Canvas c, Paint p,
			ydPoint p1, ydPoint p2, ydPoint minOverLapSlope) {
		ArrayList<ydPoint> projections = new ArrayList<ydPoint>();

		ydPoint normalSlope = negRecip(slopeOf(p1, p2));
		ydPoint orthogonal = negRecip(normalSlope);

		ydPoint lmt1 = new ydPoint();
		ydPoint lmt2 = new ydPoint();

		bodyProjectionsFor(B, lmt2, normalSlope, orthogonal);
		bodyProjectionsFor(A, lmt1, normalSlope, orthogonal);

		
		 * projections.add(linesIntectAt(new ydLine(normalSlope, new
		 * ydPoint(240, 400)), new ydLine(orthogonal, A.position)));
		 * projections.add(linesIntectAt(new ydLine(normalSlope, new
		 * ydPoint(240, 400)), new ydLine(orthogonal, A.p2)));
		 * projections.add(linesIntectAt(new ydLine(normalSlope, new
		 * ydPoint(240, 400)), new ydLine(orthogonal, A.p3)));
		 * projections.add(linesIntectAt(new ydLine(normalSlope, new
		 * ydPoint(240, 400)), new ydLine(orthogonal, A.p4)));
		 * 
		 * projections.add(linesIntectAt(new ydLine(normalSlope, new
		 * ydPoint(240, 400)), new ydLine(orthogonal, B.position)));
		 * projections.add(linesIntectAt(new ydLine(normalSlope, new
		 * ydPoint(240, 400)), new ydLine(orthogonal, B.p2)));
		 * projections.add(linesIntectAt(new ydLine(normalSlope, new
		 * ydPoint(240, 400)), new ydLine(orthogonal, B.p3)));
		 * projections.add(linesIntectAt(new ydLine(normalSlope, new
		 * ydPoint(240, 400)), new ydLine(orthogonal, B.p4)));
		 

		
		 * if (normalSlope.x < 0 && normalSlope.y < 0) // c.drawLine(240, 400,
		 * normalSlope.x*1000, normalSlope.y*1000, p); (new ydLine(normalSlope,
		 * new ydPoint(250, 400))).drawLine(c, p); else if (normalSlope.x > 0 &&
		 * normalSlope.y < 0) { // c.drawLine(240, 400, normalSlope.x*1000,
		 * normalSlope.y*1000, p); (new ydLine(normalSlope, new ydPoint(250,
		 * 400))).drawLine(c, p); } else if (normalSlope.x < 0 && normalSlope.y
		 * > 0) { // c.drawLine(240, 400, normalSlope.x*1000,
		 * normalSlope.y*1000, p); (new ydLine(normalSlope, new ydPoint(250,
		 * 400))).drawLine(c, p); } else if (normalSlope.x > 0 && normalSlope.y
		 * > 0) { // c.drawLine(240, 400, normalSlope.x*1000,
		 * normalSlope.y*1000, p); (new ydLine(normalSlope, new ydPoint(250,
		 * 400))).drawLine(c, p); } Paint paint2=new Paint();
		 * paint2.setColor(Color.BLUE); int i=0; for (ydPoint pro : projections)
		 * { pro.drawPoint(c, p); // c.drawText(pro.toString(), 20, 200, p);
		 * if(i<=3) (new ydLine(normalSlope, new ydPoint(250, 400))).drawLine(c,
		 * p); else (new ydLine(normalSlope, new ydPoint(250, 400))).drawLine(c,
		 * paint2); //c.drawText(pro.toString(), 50, 50+i*10, paint2);
		 * 
		 * i++; }
		 

		if ((lmt1.x < lmt2.x && lmt2.x < lmt1.y)
				|| (lmt1.x < lmt2.y && lmt2.y < lmt1.y)
				|| (lmt2.x < lmt1.x && lmt1.x < lmt2.y)
				|| (lmt2.x < lmt1.y && lmt1.y < lmt2.y)) {
			p.setColor(Color.RED);

			float minOverLap = Math.min(
					Math.min(Math.abs(lmt1.x - lmt2.x),
							Math.abs(lmt1.x - lmt2.y)),
					Math.min(Math.abs(lmt2.x - lmt2.x),
							Math.abs(lmt2.x - lmt2.y)));

			if (minOverLap <= minOverLapSlope.x) {
				minOverLapSlope.x = minOverLap;
				minOverLapSlope.y = vectorToDegrees(normalSlope);
			}

			return false;
		} else {
			p.setColor(Color.GREEN);
			return true;
		}

		// return;

	}*/

	private static Boolean SAT(RigidBody A, RigidBody B, ydPoint p1,
			ydPoint p2, ydPoint minOverLapSlope) {
		ArrayList<ydPoint> projections = new ArrayList<ydPoint>();

		ydPoint normalSlope = negRecip(slopeOf(p1, p2));
		ydPoint orthogonal = negRecip(normalSlope);

		ydPoint lmt1 = new ydPoint();
		ydPoint lmt2 = new ydPoint();

		bodyProjectionsFor(B, lmt2, normalSlope, orthogonal);
		bodyProjectionsFor(A, lmt1, normalSlope, orthogonal);

		if ((lmt1.x < lmt2.x && lmt2.x < lmt1.y)
				|| (lmt1.x < lmt2.y && lmt2.y < lmt1.y)
				|| (lmt2.x < lmt1.x && lmt1.x < lmt2.y)
				|| (lmt2.x < lmt1.y && lmt1.y < lmt2.y)) {
			float minOverLap = Math.min(
					Math.min(Math.abs(lmt1.x - lmt2.x),
							Math.abs(lmt1.x - lmt2.y)),
					Math.min(Math.abs(lmt2.x - lmt2.x),
							Math.abs(lmt2.x - lmt2.y)));

			if (minOverLap <= minOverLapSlope.x) {
				minOverLapSlope.x = minOverLap;
				minOverLapSlope.y = vectorToDegrees(normalSlope);
			}

			return false;
		} else {
			return true;
		}

	}

	public static void bodyProjectionsFor(RigidBody A, ydPoint lmt,
			ydPoint normalSlope, ydPoint orthogonal) {

		ydPoint proj = linesIntectAt(new ydLine(normalSlope, new ydPoint()),
				new ydLine(orthogonal, A.position));

		lmt.x = hypotenuse(new ydPoint(), proj);
		lmt.y = hypotenuse(new ydPoint(), proj);

		ydLine normLine;

		if (normalSlope.x <= 0 && normalSlope.y <= 0)
			normLine = new ydLine(normalSlope, new ydPoint(500, 500));
		else if (normalSlope.x >= 0 && normalSlope.y <= 0) {
			normLine = new ydLine(normalSlope, new ydPoint(0, 500));
		} else if (normalSlope.x <= 0 && normalSlope.y >= 0) {
			normLine = new ydLine(normalSlope, new ydPoint(500, 0));
		} else if (normalSlope.x >= 0 && normalSlope.y >= 0) {
			normLine = new ydLine(normalSlope, new ydPoint(0, 0));
		}

		proj = linesIntectAt(new ydLine(normalSlope, new ydPoint()),
				new ydLine(orthogonal, A.p2));
		setMinMax(proj, lmt);

		proj = linesIntectAt(new ydLine(normalSlope, new ydPoint()),
				new ydLine(orthogonal, A.p3));
		setMinMax(proj, lmt);

		proj = linesIntectAt(new ydLine(normalSlope, new ydPoint()),
				new ydLine(orthogonal, A.p4));
		setMinMax(proj, lmt);

	}

	public static void setMinMax(ydPoint proj, ydPoint lmt) {
		float length = hypotenuse(new ydPoint(), proj);
		if (length < lmt.x) {
			lmt.x = length;
		}
		if (length > lmt.y) {
			lmt.y = length;
		}
	}

	public static ydPoint slopeOf(ydPoint p1, ydPoint p2) {
		return new ydPoint((p1.x - p2.x) / hypotenuse(p1, p2), (p1.y - p2.y)
				/ hypotenuse(p1, p2));
	}

	public static ydPoint negRecip(ydPoint s) {
		return new ydPoint(-s.y, s.x);
	}

	public static float hypotenuse(ydPoint p1, ydPoint p2) {
		return (float) Math.hypot(p1.x - p2.x, p1.y - p2.y);
	}

	public static ydPoint linesIntectAt(ydLine l1, ydLine l2) {

		/*
		 * if(l1.slope.x-l2.slope.x==0 || l1.slope.y-l2.slope.y==0) return null;
		 */

		/*
		 * float t1= (1)/(l1.slope.x-l2.slope.x); float t2=
		 * (1)/(l1.slope.y-l2.slope.y);
		 */

		/*
		 * float r = ((l2.slope.x/l1.slope.x)*l1.slope.y)/l2.slope.y; float t =
		 * (l2.slope.x/l1.slope.x)*r;
		 */

		float t;
		float r;
		if (l1.slope.x == 0) {
			r = (l1.p1.x - l2.p1.x) / l2.slope.x;
		} else {
			r = (l2.p1.y - l1.p1.y + (l1.p1.x - l2.p1.x)
					* l1.slope.y / l1.slope.x)
					/ (l1.slope.y * l2.slope.x / l1.slope.x - l2.slope.y);
			t = (l2.slope.x * r + l2.p1.x - l1.p1.x) / l1.slope.x;
		}
		/*
		 * if(!((int)t1==(int)t2)){ return null; }
		 */

		return new ydPoint(l2.slope.x * r + l2.p1.x, l2.slope.y * r
				+ l2.p1.y);

	}
	
	public static float minDistanceToBodyEdge(RigidBody A, ydPoint p){
		float d=lengthFromPointToLine(p, new ydLine(slopeOf(A.position, A.p2), A.position));
		for(int i=0; i<4; i++){
		if(lengthFromPointToLine(p, new ydLine(slopeOf(A.getVertix(i), A.getVertix(i+1)), A.getVertix(i)))<d){
			d=lengthFromPointToLine(p, new ydLine(slopeOf(A.getVertix(i), A.getVertix(i+1)), A.getVertix(i)));
		}
		}
		return d;
	}
	
	public static Boolean pointInBody(RigidBody A, ydPoint p){
		if (SAT(A, p, A.position, A.p2))
			return false;
		if (SAT(A, p, A.position, A.p3))
			return false;
		if (SAT(A, p, A.p3, A.p4))
			return false;
		if (SAT(A, p, A.p2, A.p4))
			return false;
		
		return true;
		
	}
	
	public ydPoint getVertix(int i){
		switch (i){
		case 0:
			return position;
		case 1:
			return p2;
		case 2:
			return p4;
		case 3:
			return p3;	
		}
		
		return position;
	}
	
	public static float lengthFromPointToLine(ydPoint p, ydLine l){
		
		ydPoint POI=linesIntectAt(l, new ydLine(negRecip(l.slope), p));
		
		return hypotenuse(POI, p);
	}


private static Boolean SAT(RigidBody A, ydPoint p, ydPoint p1,
		ydPoint p2) {
	ArrayList<ydPoint> projections = new ArrayList<ydPoint>();

	ydPoint normalSlope = negRecip(slopeOf(p1, p2));
	ydPoint orthogonal = negRecip(normalSlope);

	ydPoint lmt1 = new ydPoint();
	ydPoint lmt2 = new ydPoint();

	bodyProjectionsFor(p,lmt2,normalSlope, orthogonal);
	bodyProjectionsFor(A, lmt1, normalSlope, orthogonal);
	
	float pLength=lmt2.x;
	
	if (lmt1.x < pLength && pLength < lmt1.y) {
		/*float minOverLap = Math.min(
				Math.min(Math.abs(lmt1.x - lmt2.x),
						Math.abs(lmt1.x - lmt2.y)),
				Math.min(Math.abs(lmt2.x - lmt2.x),
						Math.abs(lmt2.x - lmt2.y)));

		if (minOverLap <= minOverLapSlope.x) {
			minOverLapSlope.x = minOverLap;
			minOverLapSlope.y = vectorToDegrees(normalSlope);
		}*/

		return false;
	} else {
		return true;
	}

}

public static void bodyProjectionsFor(ydPoint p,ydPoint lmt, ydPoint normalSlope, ydPoint orthogonal) {

	ydPoint proj = linesIntectAt(new ydLine(normalSlope, new ydPoint()),
			new ydLine(orthogonal, p));


	ydLine normLine;

	if (normalSlope.x <= 0 && normalSlope.y <= 0)
		normLine = new ydLine(normalSlope, new ydPoint(500, 500));
	else if (normalSlope.x >= 0 && normalSlope.y <= 0) {
		normLine = new ydLine(normalSlope, new ydPoint(0, 500));
	} else if (normalSlope.x <= 0 && normalSlope.y >= 0) {
		normLine = new ydLine(normalSlope, new ydPoint(500, 0));
	} else if (normalSlope.x >= 0 && normalSlope.y >= 0) {
		normLine = new ydLine(normalSlope, new ydPoint(0, 0));
	}

	proj = linesIntectAt(new ydLine(normalSlope, new ydPoint()),
			new ydLine(orthogonal, p));
	lmt.x = hypotenuse(new ydPoint(), proj);
	lmt.y = hypotenuse(new ydPoint(), proj);

/*		proj = linesIntectAt(new ydLine(normalSlope, new ydPoint()),
			new ydLine(orthogonal, A.p3));
	setMinMax(proj, lmt);

	proj = linesIntectAt(new ydLine(normalSlope, new ydPoint()),
			new ydLine(orthogonal, A.p4));
	setMinMax(proj, lmt);*/

}
	
public static Boolean POCTest(RigidBody A, RigidBody B, Graphics g){
		
		
		ArrayList<ydPoint> PsOIA=new ArrayList<ydPoint>();
		ArrayList<ydPoint> PsOIB=new ArrayList<ydPoint>();
		
		for(int i=0; i<4;i++){
			if(RigidBody.pointInBody(A, B.getVertix(i))){
				PsOIA.add(B.getVertix(i));
			}
		}
		
		for(int i=0; i<4;i++){
			if(RigidBody.pointInBody(B, A.getVertix(i))){
				PsOIB.add(A.getVertix(i));
			}
		}
		
		if(PsOIA.size()==0 && PsOIB.size()==0){
			return true;
		}
		
		float minInA=1000000;
		float minInB=1000000;
		
		float d;
		for (ydPoint p: PsOIA){
			//p.drawPoint(canvas, paint2);
			d=minDistanceToBodyEdge(A, p);
			if(d<minInA)
				minInA=d;
		}
		
		for (ydPoint p: PsOIB){
			//p.drawPoint(canvas, paint2);
			d=minDistanceToBodyEdge(B, p);
			if(d<minInB)
				minInB=d;
		}
		

		
		RigidBody r;
		ArrayList<ydPoint> PsOI;
		
		if(minInA<minInB){
			r=A;
			PsOI=PsOIA;
		}
		else{
			r=B;
			PsOI=PsOIB;
		}
		
		for (ydPoint p: PsOI){
			p.drawPoint(g);
		}
		

		
		
		ydPoint slope =slopeOf(A.velocity, B.velocity);
		
		if( Float.isNaN(slope.x) ||  Float.isNaN(slope.y)){
			return true;
		}
		
		
		ydPoint SoI = new ydPoint();
		
		ydLine dofV=new ydLine(slope, new ydPoint(100, 100));
		dofV.drawLine(g);
		
		ydPoint minPoI=new ydPoint(1110, 1110);
		float minLength=1110;
		ydPoint PoI =new ydPoint() , forP=new ydPoint() ;
		for( ydPoint p:PsOI){
			for(int i=0; i<3; i++){
				ydLine l1=new ydLine(slope, p);
				ydLine l2=new ydLine(slopeOf(r.getVertix(i),r.getVertix(i+1)), r.getVertix(i));
				l1.drawLine(g);
				l2.drawLine(g);
				
				PoI=linesIntectAt(new ydLine(slope, p), new ydLine(slopeOf(r.getVertix(i),r.getVertix(i+1)), r.getVertix(i)));
				
				float h1=hypotenuse(new ydPoint(), r.getVertix(i));
				float h2=hypotenuse(new ydPoint(), r.getVertix(i+1));
				
				float h=hypotenuse(new ydPoint(), PoI);
				
				if((h>h1 && h>h2) || (h<h1&&h<h2))
					continue;
				
				h=hypotenuse(p, PoI);
					
				if(h<minLength)
				{
					minLength=h;
				//	if(hypotenuse(p, minPoI)<h){
						minPoI=new ydPoint(PoI);
						forP=p;
						SoI=slope;
				//	}
				}
			}
		}
		
		/*Paint paint3=new Paint();
		paint3.setColor(Color.YELLOW);*/
		
		minPoI.drawPoint(g);
		
		ydPoint shift=new ydPoint(forP.x- minPoI.x, forP.y- minPoI.y);
		
		if(hypotenuse(forP, minPoI)<2)
			return true;
		
		ydPoint newVelocity =new ydPoint();
		newVelocity.x=(A.velocity.x*A.MASS+B.velocity.x*B.MASS)/(A.MASS+B.MASS);
		newVelocity.y=(A.velocity.y*A.MASS+B.velocity.y*B.MASS)/(A.MASS+B.MASS);
		
		float totalVelocity=hypotenuse(new ydPoint(), newVelocity);
		/*A.velocity=new ydPoint();*/
	//	B.velocity.x=SoI.x*totalVelocity;
		//B.velocity.y=SoI.y*totalVelocity;
		B.stopAcceleration();
		B.velocity=new ydPoint();
		//B.translateBody(shift);
		B.pivot=minPoI;
		//return minPoI;
		return false;
		
		/*float minDistance=hypotenuse(A.center_mass, B.position);
		ydPoint poc=new ydPoint(B.position);
		RigidBody BoI=A;
		
		RigidBody.pointInBody(B, A.p3);
		
		if(minDistance>hypotenuse(A.center_mass, B.p2)){
			 poc.x=B.p2.x;
			 poc.y=B.p2.y;
		}
		if(minDistance>hypotenuse(A.center_mass, B.p3)){
			 poc.x=B.p3.x;
			 poc.y=B.p3.y;
		}
		if(minDistance>hypotenuse(A.center_mass, B.p4)){
			 poc.x=B.p4.x;
			 poc.y=B.p4.y;
		}
		
		if(minDistance>hypotenuse(B.center_mass, A.position)){
			BoI=B;
			 poc.x=A.position.x;
			 poc.y=A.position.y;
		}
		if(minDistance>hypotenuse(B.center_mass, A.p2)){
			BoI=B;
			 poc.x=A.p2.x;
			 poc.y=A.p2.y;
		}
		if(minDistance>hypotenuse(B.center_mass, A.p3)){
			BoI=B;
			 poc.x=A.p3.x;
			 poc.y=A.p3.y;
		}
		if(minDistance>hypotenuse(B.center_mass, A.p4)){
			BoI=B;
			 poc.x=A.p4.x;
			 poc.y=A.p4.y;
		}
		
		return poc;*/
	}

public void stopAcceleration(){
	total_uniform_force.magnitude.x=0;
	total_uniform_force.magnitude.y=0;
	acceleration.x=0;
	acceleration.y=0;
}

}
